#include "io_factory.h"
#include "io_macro.h"
#include <iostream>
#include <algorithm>
namespace io_module {
IoFactory::IoFactory(
    std::shared_ptr<rclcpp::Node> node
) : node_(node)
{
  clear();
}

IoFactory::~IoFactory()
{
  for (auto &p : m_vecInterface)
  {
    delete p;
    p = nullptr;
  }
}

void IoFactory::clear()
{
    m_init_ok = false;
    m_vecInterface.clear();
}

bool IoFactory::init(IoGroup ioGroup)
{
    if (m_init_ok) return false;
    m_ioGroup = ioGroup;
    m_init_ok = sort_by_module_number();
    m_init_ok &= init_all_module();
    return m_init_ok;
}

bool IoFactory::factory_fun(ModuleParam type)
{
    bool ret = false;
    if (type.interface_type == CANOPEN_TYPE)
    {
        auto baseCan = new CanInterface(node_, type);
        m_vecInterface.push_back(baseCan);
        DEBUG_STREAM_OUT("factory_fun:creat canopenio");
        ret = true;
    } else if (type.interface_type == MODBUS_TYPE) {
        auto baseModus = new ModbusInterface(node_, type);
        m_vecInterface.push_back(baseModus);
        DEBUG_STREAM_OUT("factory_fun:creat modbusio");
        ret = true;
    }
    else
    {
        DEBUG_ERROR_OUT("error:io factory_fun unkown type=" << type.interface_type.c_str());
        ret = false;
    }
    return ret;
}

bool IoFactory::sort_by_module_number()
{
    bool ret = true;
    try
    {
        std::sort(m_vecInterface.begin(), m_vecInterface.end(), 
            [](const BaseInterface* a, const BaseInterface* b) {
                return a->getModuleNumber() < b->getModuleNumber();
            });

        for (size_t i = 0; i < m_vecInterface.size(); ++i)
        {
            DEBUG_OUT("m_vecInterface[%zu]=index=%d,type=%s",
                i,
                m_vecInterface[i]->getModuleNumber(),
                m_vecInterface[i]->getModuleParam().interface_type.c_str()
            );
        }
    }
    catch (const std::exception &e)
    {
        DEBUG_ERROR_OUT("exception sort_by_module_number:" << e.what());
        ret = false;
    }
    catch (...)
    {
        ret = false;
        DEBUG_ERROR_OUT("crash sort_by_module_number");
    }
    return ret;
}

bool IoFactory::init_all_module()
{
    bool ret = true;
    for (auto &p : m_vecInterface)
    {
        if (p == nullptr)
        {
            ret = false;
            DEBUG_ERROR_OUT("error:init_all_module p is nullptr;");
            break;
        }
        bool ok = p->init();
        if (!ok)
        {
            ret = false;
            DEBUG_OUT("error:init_all_module failed; moduleNumber=%d", p->getModuleNumber());
            break;
        }
    }
    return ret;
}

bool IoFactory::read_input_ios(const int &count_io, uint64_t &rets)
{
    std::lock_guard<std::mutex> locker(m_mutex);
 
    if (!m_init_ok)
    {
        DEBUG_ERROR_OUT("IoFactory not init ok;read_input_ios failed");
        return false;
    }

    if (count_io < 0 || count_io > m_ioGroup.num_input)
    {
        DEBUG_ERROR_OUT("error ;read_input_ios  out range count_io=" << count_io);
        return false;
    }

    if (m_vecInterface.empty())
    {
        DEBUG_ERROR_OUT("error ;read_input_ios  m_vecInterface is empty");
        return false;
    }

    bool read_ok = true;
    rets = 0;
    uint8_t offset = 0;
    // 如果count_io != m_ioGroup.num_input
    // 数量不等，则可能需要分割。临时全部读取
    for (std::size_t i = 0; i < m_vecInterface.size(); i++)
    {
        // 处理某个-当前模块
        // 当前模块的所有输入
        uint64_t curBits = 0;
        // 当前模块
        auto &p = m_vecInterface[i];
        if (p == nullptr)
        {
            DEBUG_ERROR_OUT("read_input_ios p is nullptr i=" << i);
            return false;
        }
     
        // 当前模块的输入数量
        int number_input = p->getModuleParam().num_digital_input;
        bool input_nothing = (number_input == 0);
        DEBUG_WARN_ON_TRANSITION(input_nothing, "warning: current module numInput is " << number_input);
        if (input_nothing)
        {
            continue;
        }

        // 当前配置模块数量不为0-正常读取
        // 一次性读取配置的所有io输入
        read_ok &= p->readInput(curBits);
        // 偏移到当前模块指定位置例如：module1:8  module2:16 依次偏移0，8，24
        rets += curBits << offset; // 移动
        offset += number_input;
        if (!read_ok)
        {
            DEBUG_ERROR_OUT("read_input_ios failed; i=" << i);
        }
    }

    bool same = count_io == offset;
    if (!same)
    {
        DEBUG_ERROR_OUT("error:read_input_ios count is not match;count_io=" << count_io << ",offset=" << (int)(offset));
    }
    
    return read_ok;
}

bool IoFactory::set_output_io(const int &index, const bool &level)
{
    if (!m_init_ok)
    {
        DEBUG_ERROR_OUT("IoFactory not init ok;set_output_io failed");
        return false;
    }

    if (index < 0 || index >= m_ioGroup.num_output)
    {
        DEBUG_ERROR_OUT("error index: set_output_io index=" << index << ",num_output=" << m_ioGroup.num_output);
        return false;
    }

    if (m_vecInterface.empty())
    {
        DEBUG_ERROR_OUT("error ;set_output_io  m_vecInterface is empty");
        return false;
    }

    bool set_ok = false;
    uint8_t offset = 0;
    for (std::size_t i = 0; i < m_vecInterface.size(); i++)
    {
        // 当前模块
        auto &p = m_vecInterface[i];
        if (p == nullptr)
        {
            DEBUG_ERROR_OUT("set_output_io p is nullptr i=" << i);
            return false;
        }
        // 当前模块的输出数量
        uint8_t numOutput = p->getModuleParam().num_digital_output;
        // 查找索引区间在哪个模块内
        if (index < (offset + numOutput))
        {
            uint8_t moduleIndex = index - offset;
            set_ok = p->setOutput(moduleIndex, level);
            break;
        }
        offset += numOutput; // 更新偏移值，为下次循环做准备
    }
    return set_ok;
}

bool IoFactory::read_analog_inputs(const int &channel_count, std::vector<BaseInterface::WORD> &rets)
{
    std::lock_guard<std::mutex> locker(m_mutexA);
    if (!m_init_ok)
    {
        DEBUG_ERROR_OUT("IoFactory not init ok;read_analog_inputs failed");
        return false;
    }

    if (channel_count < 0 || channel_count > m_ioGroup.num_analog_input)
    {
        DEBUG_ERROR_OUT("error ;read_analog_inputs  out range count_io=" << channel_count);
        return false;
    }

    if (m_vecInterface.empty())
    {
        DEBUG_ERROR_OUT("error ;read_analog_inputs  m_vecInterface is empty");
        return false;
    }

    bool read_ok = true;
    rets.clear();
    // 如果count_io != m_ioGroup.num_analog_input
    // 数量不等，则可能需要分割。临时全部读取
    for (std::size_t i = 0; i < m_vecInterface.size(); i++)
    {
        // 处理某个-当前模块
        // 当前模块的所有输入
        std::vector<BaseInterface::WORD> curWords;
        // 当前模块
        auto &p = m_vecInterface[i];
        if (p == nullptr)
        {
            DEBUG_ERROR_OUT("read_analog_inputs p is nullptr i=" << i);
            return false;
        }
        // 当前模块的模拟量输入数量
        uint8_t numInputA = p->getModuleParam().num_analog_input;
        bool is_zero = numInputA == 0;
        DEBUG_WARN_ON_TRANSITION(is_zero, "warning: current module numInputA is 0");
        if (numInputA == 0)
        {
            // ROS_WARN_STREAM_THROTTLE(0.2, "warning: current module numInputA is 0");
            continue;
        }

        // 当前配置模块数量不为0-正常读取
        // 一次性读取配置的所有模拟量输入
        read_ok &= p->readInputA(curWords);
        // 按照顺序一次放入总的缓冲区
        for(auto word:curWords)
        {
            rets.push_back(word);
        }

        if (!read_ok)
        {
            DEBUG_ERROR_OUT("read_analog_inputs read_ok failed; i =" << i);
        }
    }

    bool same = channel_count == (int)(rets.size());
    if (!same)
    {
        DEBUG_ERROR_OUT("error:read_analog_inputs count is not match;channel_count=" << channel_count << ",real offset=" << (int)(rets.size()));
    }

    return read_ok;
}

bool IoFactory::set_analog_output(const int &channel_index, const BaseInterface::WORD & raw_data)
{
    if (!m_init_ok)
    {
        DEBUG_ERROR_OUT("IoFactory not init ok;set_analog_output failed");
        return false;
    }

    if (channel_index < 0 || channel_index >= m_ioGroup.num_analog_output)
    {
        DEBUG_ERROR_OUT("error index: set_analog_output index=" << channel_index << ",num_analog_output=" << m_ioGroup.num_analog_output);
        return false;
    }

    if (m_vecInterface.empty())
    {
        DEBUG_ERROR_OUT("error ;set_analog_output  m_vecInterface is empty");
        return false;
    }

    bool set_ok = false;
    uint8_t offset = 0;
    for (std::size_t i = 0; i < m_vecInterface.size(); i++)
    {
        // 当前模块
        auto &p = m_vecInterface[i];
        if (p == nullptr)
        {
            DEBUG_ERROR_OUT("set_analog_output p is nullptr i=" << i);
            return false;
        }
        // 当前模块的输出数量
        uint8_t numOutputA = p->getModuleParam().num_analog_output;
        // 查找索引区间在哪个模块内
        if (channel_index < (offset + numOutputA))
        {
            uint8_t moduleIndex = channel_index - offset;
            set_ok = p->setOutputA(moduleIndex, raw_data);
            break;
        }
        offset += numOutputA; // 更新偏移值，为下次循环做准备
    }
    return set_ok;
}
}